/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-06 09:51:41
 * @LastEditors: EltonLi lijiaming@uavi-tech.com
 * @LastEditTime: 2025-03-22 22:16:18
 * @FilePath: \zino-fc-v4\ZINO\sensor\QMI8658\qmi8658.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-09-06     UAVI02       the first version
 */
#include "qmi8658.h"
#include "drv_i2c.h"
#include <rtthread.h>

// #define QMI8658_I2C_ADDR 0x6A
#define QMI8658_I2C_ADDR 0x6B

// static rt_err_t qmi8658_spi_write_reg(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t *data)
// {
//     return rt_spi_send_then_send(dev->spi_dev, &reg_addr, 1, data, 1);
// }

// static rt_err_t qmi8658_spi_write_regs(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t *data, uint8_t len)
// {
//     return rt_spi_send_then_send(dev->spi_dev, &reg_addr, 1, data, len);
// }

// static rt_err_t qmi8658_spi_read_reg(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t *data)
// {
//     reg_addr |= 0x80;
//     return rt_spi_send_then_recv(dev->spi_dev, &reg_addr, 1, data, 1);
// }

// static rt_err_t qmi8658_spi_read_regs(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t *data, uint8_t len)
// {
//     reg_addr |= 0x80;
//     return rt_spi_send_then_recv(dev->spi_dev, &reg_addr, 1, data, len);
// }
int qmi8658_i2c_write_reg(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t data)
{
    return soft_i2c_write(dev->i2c_addr, reg_addr, &data, 1);
}
int8_t qmi8658_i2c_read_regs(qmi8658_device_t *dev, uint8_t reg_addr, uint8_t *data, uint8_t len)
{
    return soft_i2c_read(dev->i2c_addr, reg_addr, data, len);
}
qmi8658_device_t qmi8658;
const uint16_t qmi8658_acc_scale[4] = {
    QM8658_ACC_SCALE_2G,
    QM8658_ACC_SCALE_4G,
    QM8658_ACC_SCALE_8G,
    QM8658_ACC_SCALE_16G,
};
const uint16_t qmi8658_gyro_scale[8] = {
    QM8658_GYRO_SCALE_16DPS,
    QM8658_GYRO_SCALE_32DPS,
    QM8658_GYRO_SCALE_64DPS,
    QM8658_GYRO_SCALE_128DPS,
    QM8658_GYRO_SCALE_256DPS,
    QM8658_GYRO_SCALE_512DPS,
    QM8658_GYRO_SCALE_1024DPS,
    QM8658_GYRO_SCALE_2048DPS,
};
int qmi8658_init(void)
{
    qmi8658.i2c_addr = QMI8658_I2C_ADDR << 1;
    qmi8658_i2c_read_regs(&qmi8658, QMI8658_REG_WHO_AM_I, &qmi8658.who_am_i, 1);
    rt_kprintf("qmi8658.who_am_i:0x%02X\n", qmi8658.who_am_i);
    qmi8658_i2c_read_regs(&qmi8658, QMI8658_REG_REVISON_ID, &qmi8658.device_revision_id, 1);
    rt_kprintf("qmi8658.device_revision_id:0x%02X\n", qmi8658.device_revision_id);

    qmi8658_i2c_write_reg(&qmi8658, QMI8658_REG_CTRL1, BIT6);
    qmi8658_i2c_write_reg(&qmi8658, QMI8658_REG_CTRL2, (QMI8658_CTRL2_ACC_RANGE_16G << 4 | QMI8658_ACC_ODR_NORMAL_1000Hz));
    qmi8658_i2c_write_reg(&qmi8658, QMI8658_REG_CTRL3, (QMI8658_GYRO_RANGE_512dps << 4 | QMI8658_GYRO_ODR_NORMAL_1000Hz));
    qmi8658_i2c_write_reg(&qmi8658, QMI8658_REG_CTRL7, (QMI8658_CTRL7_EnableGyro | QMI8658_CTRL7_EnableAcc));

    uint8_t reg;
    qmi8658_i2c_read_regs(&qmi8658, QMI8658_REG_CTRL2, &reg, 1);
    qmi8658.acc_range = (reg >> 4) & 0x07;
    qmi8658.acc_scale = qmi8658_acc_scale[qmi8658.acc_range % 4];
    qmi8658_i2c_read_regs(&qmi8658, QMI8658_REG_CTRL3, &reg, 1);
    qmi8658.gyro_range = (reg >> 4) & 0x07;
    qmi8658.gyro_scale = qmi8658_gyro_scale[qmi8658.gyro_range % 8];

    rt_kprintf("acc range:%d scale:%d\n", qmi8658.acc_range, qmi8658.acc_scale);
    rt_kprintf("gyro range:%d scale:%d\n", qmi8658.gyro_range, qmi8658.gyro_scale);

    return RT_EOK;
    // qmi8658_i2c_write_reg(&qmi8658, QMI8658_REG_CTRL5, 0x00);
}
float qmi8658_get_temp(void)
{
    uint8_t data[2];
    qmi8658_i2c_read_regs(&qmi8658, QMI8658_REG_TEMP_L, data, 2);
    return (float)((float)data[0] / 256.f + data[1]);
}
int8_t qmi8658_read_raw_acc_gyro(int16_t *acc, int16_t *gyro)
{
    uint8_t data[12];

    int8_t ret = qmi8658_i2c_read_regs(&qmi8658, QMI8658_REG_AX_L, data, 12);
    acc[0] = (data[0] | (data[1] << 8));
    acc[1] = (data[2] | (data[3] << 8));
    acc[2] = (data[4] | (data[5] << 8));
    gyro[0] = (data[6] | (data[7] << 8));
    gyro[1] = (data[8] | (data[9] << 8));
    gyro[2] = (data[10] | (data[11] << 8));
    return ret;
}
int qmi8658_read_acc_gyro(float *acc, float *gyro)
{
    int16_t raw_acc[3], raw_gyro[3];
    int ret = qmi8658_read_raw_acc_gyro(raw_acc, raw_gyro);
    acc[0] = (float)raw_acc[0] / (float)qmi8658.acc_scale;
    acc[1] = (float)raw_acc[1] / (float)qmi8658.acc_scale;
    acc[2] = (float)raw_acc[2] / (float)qmi8658.acc_scale;
    gyro[0] = (float)raw_gyro[0] / (float)qmi8658.gyro_scale;
    gyro[1] = (float)raw_gyro[1] / (float)qmi8658.gyro_scale;
    gyro[2] = (float)raw_gyro[2] / (float)qmi8658.gyro_scale;
    return ret;
}
// int qmi8658_i2c_write_bits(qmi8658_device_t *dev, uint8_t reg, uint8_t start_bit, uint8_t len, uint8_t data)
// {
//     rt_uint8_t byte, mask;
//     rt_err_t res = RT_EOK;

//     res = qmi8658_i2c_read_regs(dev, reg, &byte, 1);
//     if (res != RT_EOK)
//     {
//         return res;
//     }
//     mask = ((1 << len) - 1) << (start_bit - len + 1);
//     data <<= (start_bit - len + 1); // shift data into correct position
//     data &= mask;                   // zero all non-important bits in data
//     byte &= ~(mask);                // zero all important bits in existing byte
//     byte |= data;
//     return qmi8658_i2c_write_reg(dev, reg, &byte);
// }
// // static rt_err_t qmi8658_spi_write_bits(qmi8658_device_t *dev, rt_uint8_t reg, rt_uint8_t start_bit, rt_uint8_t len, rt_uint8_t data)
// // {
// //     rt_uint8_t byte, mask;
// //     rt_err_t res = RT_EOK;

// //     res = qmi8658_spi_read_regs(dev, reg, &byte, 1);
// //     if (res != RT_EOK)
// //     {
// //         return res;
// //     }
// //     mask = ((1 << len) - 1) << (start_bit - len + 1);
// //     data <<= (start_bit - len + 1); // shift data into correct position
// //     data &= mask;                   // zero all non-important bits in data
// //     byte &= ~(mask);                // zero all important bits in existing byte
// //     byte |= data;
// //     return qmi8658_spi_write_reg(dev, reg, &byte);
// // }

// static rt_err_t qmi8658_spi_reg(qmi8658_device_t *dev, uint8_t cmd, uint16_t param)
// {
//     rt_err_t res = RT_EOK;
//     switch (cmd)
//     {
//     case QMI8658_ACC_ODR_SET:
//         res = qmi8658_i2c_write_bits(dev, QMI8658_REG_CTRL2, 3, 4, param);
//         break;
//     case QMI8658_ACC_RANGE_SET:
//         res = qmi8658_i2c_write_bits(dev, QMI8658_REG_CTRL2, 6, 3, param);
//         dev->range.acce_range = param;
//         rt_kprintf("sen_acce = %d\n", dev->range.acce_range);
//         break;

//     case QMI8658_GYRO_ODR_SET:
//         res = qmi8658_i2c_write_bits(dev, QMI8658_REG_CTRL3, 3, 4, param);
//         break;
//     case QMI8658_GYRO_RANGE_SET:
//         res = qmi8658_i2c_write_bits(dev, QMI8658_REG_CTRL3, 6, 4, param);
//         dev->range.gyro_range = param;
//         rt_kprintf("sen_gyro = %d\n", dev->range.gyro_range);
//         break;
//     case QMI8658_ALPF_SET:
//         res = qmi8658_i2c_write_bits(dev, QMI8658_REG_CTRL5, 4, 1, 1);
//         res = qmi8658_i2c_write_bits(dev, QMI8658_REG_CTRL5, 6, 2, param);
//         break;
//     case QMI8658_GLPF_SET:
//         res = qmi8658_i2c_write_bits(dev, QMI8658_REG_CTRL5, 0, 1, 1);
//         res = qmi8658_i2c_write_bits(dev, QMI8658_REG_CTRL5, 2, 2, param);
//         break;
//     case QMI8658_MODE_SET:
//         res = qmi8658_spi_write_reg(dev, QMI8658_REG_CTRL7, param);
//         break;
//     default:
//         break;
//     }
//     return res;
// }

// static rt_err_t qmi8658_init(qmi8658_device_t *dev, const char *bus_name)
// {
//     rt_uint8_t res = RT_EOK;
//     res = qmi8658_spi_init(dev);
//     if (res != RT_EOK)
//     {
//         LOG_E("qmi8658 find spi device failed");
//         return -RT_ERROR;
//     }
//     qmi8658_spi_read_regs(dev, QMI8658_REG_WHO_AM_I, &dev->dev_id, 1);
//     if (dev->dev_id != QMI8658_DEFAULT_ID)
//     {
//         LOG_E("qmi8658 read id error, rx: 0x%X, expect: 0x%X", dev->dev_id, QMI8658_DEFAULT_ID);
//         return -RT_ERROR;
//     }
//     else
//     {
//         LOG_D("qmi8658 read id success, rx:0x%x, expect:0x%x", dev->dev_id, QMI8658_DEFAULT_ID);
//         // res += qmi8658_spi_write_reg(dev, QMI8658_REG_CTRL1, 64);
//         res += qmi8658_spi_write_reg(dev, QMI8658_REG_CTRL7, QMI8658_CTRL7_DisableSyncSmpleMode | QMI8658_CTRL7_HighSpeedInternalClock | QMI8658_CTRL7_GyroFullMode | QMI8658_CTRL7_DisalbeAttitudeEngine | QMI8658_CTRL7_DisableMagnetmoeter | QMI8658_CTRL7_EnableGyro | QMI8658_CTRL7_EnableAcc);
//     }
//     if (res == RT_EOK)
//     {
//         LOG_I("Device init succeed!");
//     }
//     else
//     {
//         LOG_E("Error in device initialization!");
//     }
//     return res;
// }

// static rt_err_t qmi8658_get_acc_raw(qmi8658_device_t *dev)
// {
//     rt_err_t res;
//     rt_uint8_t buffer[6];
//     res = qmi8658_spi_read_regs(dev, QMI8658_REG_AX_L, buffer, 6);
//     if (res != RT_EOK)
//         return res;
//     dev->acce_raw.x = (buffer[1] << 8) | buffer[0];
//     dev->acce_raw.y = (buffer[3] << 8) | buffer[2];
//     dev->acce_raw.z = (buffer[5] << 8) | buffer[4];
//     return RT_EOK;
// }

// static rt_err_t qmi8658_get_gyro_raw(qmi8658_device_t *dev)
// {
//     rt_err_t res;
//     rt_uint8_t buffer[6];
//     res = qmi8658_spi_read_regs(dev, QMI8658_REG_GX_L, buffer, 6);
//     if (res != RT_EOK)
//         return res;
//     dev->gyro_raw.x = (buffer[1] << 8) | buffer[0];
//     dev->gyro_raw.y = (buffer[3] << 8) | buffer[2];
//     dev->gyro_raw.z = (buffer[5] << 8) | buffer[4];
//     return RT_EOK;
// }

// static qmi8658_device_t *_qmi8658_init(struct rt_sensor_intf *intf)
// {
//     qmi8658_device_t *qmi8658 = RT_NULL;
//     uint8_t res;
//     qmi8658 = rt_calloc(1, sizeof(qmi8658_device_t));
//     res = qmi8658_init(qmi8658, intf->dev_name);
//     if (res == RT_EOK)
//         return qmi8658;
//     else
//         return RT_NULL;
// }

// static rt_size_t qmi8658_fetch_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len)
// {
//     qmi8658_device_t *qmi8658 = (qmi8658_device_t *)sensor->parent.user_data;
//     struct rt_sensor_data *data = (struct rt_sensor_data *)buf;
//     if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
//     {
//         if (qmi8658_get_acc_raw(qmi8658) != RT_EOK)
//             return 0;
//         data->type = RT_SENSOR_CLASS_ACCE;
//         data->data.acce.x = qmi8658->acce.x;
//         data->data.acce.y = qmi8658->acce.y;
//         data->data.acce.z = qmi8658->acce.z;
//         data->timestamp = rt_sensor_get_ts();
//     }
//     else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
//     {
//         if (qmi8658_get_acc_raw(qmi8658) != RT_EOK)
//             return 0;
//         data->type = RT_SENSOR_CLASS_GYRO;
//         data->data.gyro.x = qmi8658->gyro.x * 1000;
//         data->data.gyro.y = qmi8658->gyro.y * 1000;
//         data->data.gyro.z = qmi8658->gyro.z * 1000;
//         data->timestamp = rt_sensor_get_ts();
//     }
//     return 1;
// }

// static rt_err_t qmi8658_control(struct rt_sensor_device *sensor, int cmd, void *args)
// {
//     rt_err_t res = RT_EOK;
//     qmi8658_device_t *qmi8658 = sensor->parent.user_data;
//     switch (cmd)
//     {
//     case RT_SENSOR_CTRL_SET_ODR:
//         if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
//         {
//             res = qmi8658_spi_set_reg(qmi8658, QMI8658_ACC_ODR_SET, (rt_uint32_t)args);
//         }
//         else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
//         {
//             res = qmi8658_spi_set_reg(qmi8658, QMI8658_GYRO_ODR_SET, (rt_uint32_t)args);
//         }
//         break;
//     case RT_SENSOR_CTRL_SET_RANGE:
//         if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
//         {
//             res = qmi8658_spi_set_reg(qmi8658, QMI8658_ACC_RANGE_SET, (rt_uint32_t)args);
//         }
//         else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
//         {
//             res = qmi8658_spi_set_reg(qmi8658, QMI8658_GYRO_RANGE_SET, (rt_uint32_t)args);
//         }
//         break;
//     default:
//         break;
//     }
//     return res;
// }

// static struct rt_sensor_ops qmi8658_ops =
//     {
//         qmi8658_fetch_data,
//         qmi8658_control};

// rt_err_t rt_hw_qmi8658_init(const char *name, struct rt_sensor_config *cfg)
// {
//     rt_uint8_t res = RT_EOK;
//     rt_sensor_t qmi_acce = RT_NULL, qmi_gyro = RT_NULL;
//     qmi8658_device_t *QMI8658 = _qmi8658_init(&cfg->intf);
//     if (QMI8658 == RT_NULL)
//     {
//         LOG_E("_qmi8658 init err!");
//         return -RT_ERROR;
//     }
//     qmi_acce = rt_calloc(1, sizeof(struct rt_sensor_device));
//     if (qmi_acce == RT_NULL)
//         return -RT_ERROR;
//     qmi_acce->info.type = RT_SENSOR_CLASS_ACCE;
//     qmi_acce->info.vendor = RT_SENSOR_VENDOR_STM;
//     qmi_acce->info.model = "qmi8658_acc";
//     qmi_acce->info.unit = RT_SENSOR_UNIT_MG;
//     qmi_acce->info.intf_type = RT_SENSOR_INTF_SPI;
//     qmi_acce->info.period_min = 10;
//     qmi_acce->info.fifo_max = 0;

//     rt_memcpy(&qmi_acce->config, cfg, sizeof(struct rt_sensor_config));
//     qmi_acce->ops = &qmi8658_ops;
//     res = rt_hw_sensor_register(qmi_acce, name, RT_DEVICE_FLAG_RDWR, QMI8658);
//     if (res != RT_EOK)
//     {
//         LOG_E("device register err code: %d", res);
//         rt_free(qmi_acce);
//         return -RT_ERROR;
//     }
//     LOG_I("lsm6dsr acc device init success");

//     qmi_gyro = rt_calloc(1, sizeof(struct rt_sensor_device));
//     if (qmi_gyro == RT_NULL)
//         return -RT_ERROR;

//     qmi_gyro->info.type = RT_SENSOR_CLASS_GYRO;
//     qmi_gyro->info.vendor = RT_SENSOR_VENDOR_STM;
//     qmi_gyro->info.model = "qmi8658_gyro";
//     qmi_gyro->info.unit = RT_SENSOR_UNIT_MDPS;
//     qmi_gyro->info.intf_type = RT_SENSOR_INTF_SPI;
//     qmi_gyro->info.period_min = 10;
//     qmi_gyro->info.fifo_max = 0;
//     rt_memcpy(&qmi_gyro->config, cfg, sizeof(struct rt_sensor_config));
//     qmi_gyro->ops = &qmi8658_ops;
//     res = rt_hw_sensor_register(qmi_gyro, name, RT_DEVICE_FLAG_RDWR, QMI8658);
//     if (res != RT_EOK)
//     {
//         LOG_E("device register err code: %d", res);
//         rt_free(qmi_gyro);
//         return -RT_ERROR;
//     }
//     LOG_I("qmi8658 gyro device success");
//     return RT_EOK;
// }

// int rt_hw_qmi8658_port(void)
// {
//     struct rt_sensor_config cfg;
//     cfg.intf.dev_name = QMI8658_DEVICE_NAME;
//     cfg.intf.type = RT_Device_Class_SPIBUS;
//     // cfg.intf.user_data = ;
//     cfg.irq_pin.pin = RT_PIN_NONE;
//     return rt_hw_qmi8658_init("qmi", &cfg);
// }

// // ZINO_APP_EXPORT(rt_hw_qmi8658_port);

// void rt_hw_qmi8658_data(void)
// {
//     rt_device_t qmi_acce = RT_NULL, qmi_gyro = RT_NULL;
//     struct rt_sensor_data qmi_acce_data, qmi_gyro_data;
//     qmi_acce = rt_device_find("acce_qmi");
//     qmi_gyro = rt_device_find("gyro_qmi");
//     rt_device_open(qmi_acce, RT_DEVICE_FLAG_RDWR);
//     rt_device_open(qmi_gyro, RT_DEVICE_FLAG_RDWR);
//     rt_device_control(qmi_acce, RT_SENSOR_CTRL_SET_RANGE, QMI8658_CTRL2_ACC_RANGE_4G);
//     rt_device_control(qmi_gyro, RT_SENSOR_CTRL_SET_RANGE, QMI8658_GYRO_RANGE_256dps);
//     rt_device_control(qmi_acce, RT_SENSOR_CTRL_SET_ODR, QMI8658_ACC_ODR_NORMAL_1000Hz);
//     rt_device_control(qmi_gyro, RT_SENSOR_CTRL_SET_ODR, QMI8658_GYRO_ODR_NORMAL_1000Hz);
//     while (1)
//     {
//         rt_device_read(qmi_acce, 0, &qmi_acce_data, 1);
//         rt_device_read(qmi_gyro, 0, &qmi_gyro_data, 1);
//         rt_kprintf("acce: x:%5d, y:%5d, z:%5d \t", qmi_acce_data.data.acce.x, qmi_acce_data.data.acce.y, qmi_acce_data.data.acce.z);
//         rt_kprintf("gyro: x:%5d, y:%5d, z:%5d, timestamp:%5d\n", qmi_gyro_data.data.gyro.x, qmi_gyro_data.data.gyro.y, qmi_gyro_data.data.gyro.z, qmi_gyro_data.timestamp);
//         rt_thread_mdelay(10);
//     }
// }
// MSH_CMD_EXPORT(rt_hw_qmi8658_data, get qmi8658 data);
